classdef RxTrackingLoop < matlab.System
    % 旋转载体多天线卫星导航接收机跟踪环路设计
    % 负责五个环路(旋转载波成分跟踪环路、旋转频率跟踪环路、多天线合并环路、码环、非旋转非旋转载波成分跟踪环路)
    % 这一部分参考 HelperTracker的思路, 但最终表现是上述五个环路的集合。
    %  目前的计划: 
    % 1. 非旋转载波成分跟踪环路: 完成简单的单天线 无旋转效应 无平动多普勒效应 无码延迟 无噪声下的 信号解调 (实际设计的调制是存在的，但设置为0)
    % 2. 旋转频率跟踪环路: 完成多天线 旋转相位完美估计下的 旋转频率跟踪环路， see RotatingFrequencyTracker.m
    
    properties (Nontunable)

        bitDuration 
        codeRate 
        codeLen 
        CenterFrequency
        SampleRate

        IntegTime % 总相关积分时间
        T_coh % 以T_coh 为周期的积分清除器
        T_roll % 以T_roll 为周期的积分清除器

        InitialCodePhaseOffset
        InitialCarrierPhaseOffset
        InitialCarrierFreqOffset
        InitialRotFreqOffset

        pPLLLockDetector
        pHelperCNDREstimator
        pFreqDiscriminator
        PLLFilter
        FLLFilter

        pCodeTrackingLoop
        pNonRotatingCarrierTracker % 可能不需要?
        pRotatingCarrierPartTracker
        pRotatingFrequencyTracker
        pMultiAntennaCombiner

        rotPhaseModel % 旋转对载波相位的影响模型

        antennaNum
    end

    properties (Access = private)
        % Pre-computed constants
        numCBPerSym
        numSamplesPerCB
        numSamplesPerChip
        dequeSizeInit

        % Input sample buffers/deques
        deque
        deque2

        % 估计的载波相位、频率、旋转频率
        cph
        fqy
        ph
        phCF
        rotFreq  % 旋转频率估计
        rotPhase % 旋转相位估计
        rotCarrierPartEst = struct('A', 0, 'alpha', 0); % 旋转载波成分估计


        % Tracking loop time
        tFrameStart
        tFrameStartPrev
        tRxInit

    end

    methods (Access = public)
        function obj = RxTrackingLoop(config, rxConfig)

            obj.bitDuration = config.signal.bitDuration;
            obj.codeRate = config.signal.codeRate;
            obj.codeLen = config.signal.codeLen;
            obj.CenterFrequency = config.signal.CenterFrequency;
            obj.SampleRate = config.signal.SampleRate;
            obj.IntegTime = rxConfig.tracker.totalIntegTime;
            obj.tRxInit = 0;
            
            % 旋转频率跟踪环路部分参数
            obj.T_coh = rxConfig.tracker.rotationFreq.T_coh;
            obj.T_roll = rxConfig.tracker.rotationFreq.T_roll;

            % 初始相位、频率、旋转频率偏移等等
            obj.InitialCodePhaseOffset = rxConfig.tracker.dll.InitialCodePhaseOffset;  % 直接使用码片数作为初始偏移
            obj.InitialCarrierPhaseOffset = 0;
            obj.InitialCarrierFreqOffset = rxConfig.tracker.fll.InitialFreqOffset;
            obj.InitialRotFreqOffset = rxConfig.tracker.rotationFreq.InitialFreqOffset;
            
            % 器件参数
            obj.pPLLLockDetector = HelperPLLLockDetector(rxConfig);
            obj.pHelperCNDREstimator = HelperCNDREstimatorNWPR(config, rxConfig);
            obj.pFreqDiscriminator = HelperFreqDiscriminator(rxConfig.module.freqDiscriminator.type, ...
                rxConfig.module.freqDiscriminator.FreqDiscriminatorToZero, ...
                rxConfig.module.freqDiscriminator.PhaseDiscriminatorToZero);
            obj.PLLFilter = ...
                HelperLoopFilter(rxConfig.tracker.pll.noiseBandwidth, ...
                rxConfig.tracker.pll.order, rxConfig.tracker.pll.flagBoxCarInteg);    
            obj.FLLFilter = ...
                HelperLoopFilter(rxConfig.tracker.fll.noiseBandwidth, ...
                rxConfig.tracker.fll.order, rxConfig.tracker.fll.flagBoxCarInteg);

            % 环路
            obj.pMultiAntennaCombiner = MultiAntennaCombiner(config, rxConfig);
            obj.pCodeTrackingLoop = CodeTrackingLoop(config, rxConfig);
            % obj.pNonRotatingCarrierTracker = NonRotatingCarrierTracker(config, rxConfig);
            obj.pRotatingCarrierPartTracker = RotatingCarrierPartTracker(config, rxConfig);
            obj.pRotatingFrequencyTracker = RotatingFrequencyTracker(config, rxConfig);

            % 计算用的其他子函数 
            % 旋转对载波相位的影响模型（近似模型） ref $$ A_R \cos\left[2\pi \left(f_R t + \frac{m}{M}\right) + \alpha_R\right]$$
            obj.rotPhaseModel = @(t, rotFreqEst, rotCarrierPartEst) getRotPhaseEst(t, rotFreqEst, rotCarrierPartEst, obj.antennaNum);

            %  各类参数检查
            % 天线数量check
            % if config.antennas.count ~= rxConfig.receiver.antennaNum
            %     error('天线数量不一致');
            % end
            obj.antennaNum = config.antennas.count;

            % 相干积分时间检查
            if mod(obj.IntegTime, obj.T_coh) ~= 0
                error('总相干积分时间应该为T_coh相干时间的整数倍');
            end

            % 检查 旋转频率跟踪环路的Tcoh T_roll是否满足条件
            N0 = config.noise.boltzmann * config.noise.temperature * config.noise.bandwidth;
            obj.pRotatingFrequencyTracker.checkTcoh(obj.InitialRotFreqOffset, obj.bitDuration, 1, N0);
            % obj.pRotatingFrequencyTracker.checkTcoh(obj.InitialRotFreqOffset, obj.bitDuration);
            obj.pRotatingFrequencyTracker.checkTroll(obj.InitialRotFreqOffset);
        end

    end

    methods (Static)
        function integ_coh_p = integCohP(iq_p, T_coh, SampleRate)
            % 以$T_{coh}$ 为周期的积分清除器, 等于对iq_p分段求和
            % len1 * 天线数量 -> len2 * ? *  天线数量: 这里缺失了一点数据, 但是不影响后续处理?
            len1 = size(iq_p, 1);
            len2 = ceil(len1 / (T_coh * SampleRate));
            integ_coh_p = reshape(sum(reshape(iq_p, [len2, T_coh * SampleRate, size(iq_p, 2)])), [len2, 1, size(iq_p, 2)]);
        end
    end

    methods (Access = protected)    
        function resetImpl(obj)
            % Initialize / reset discrete-state properties
            obj.cph = obj.dequeSizeInit / obj.numSamplesPerChip;
            obj.fqy = obj.InitialCarrierFreqOffset;
            obj.ph = obj.InitialCarrierPhaseOffset;
            obj.phCF = 0;
            obj.rotFreq = obj.InitialRotFreqOffset;


            obj.tFrameStart = obj.tRxInit - obj.dequeSizeInit / obj.SampleRate;
            obj.tFrameStartPrev = obj.tFrameStart - obj.IntegTime / 2;

            obj.deque.reset();
            obj.deque2.reset();

            obj.FLLFilter.reset();
            obj.PLLFilter.reset();
            obj.pFreqDiscriminator.reset();
            obj.pPLLLockDetector.reset();
            obj.pHelperCNDREstimator.reset();

            obj.pCodeTrackingLoop.reset();
            obj.pRotatingCarrierPartTracker.reset();
            obj.pRotatingFrequencyTracker.reset();
            obj.pMultiAntennaCombiner.reset();
            % obj.pNonRotatingCarrierTracker.reset();
        end

        function setupImpl(obj)
            % 计算每符号包含的码块速率
            CBRate = obj.codeRate / obj.codeLen;
            % 1个数据比特持续时间内包含的码块数（整数），如GPS NAV消息通常bit duration为20ms
            obj.numCBPerSym = round(obj.bitDuration * CBRate);
            % 每个码片所对应的采样点数
            obj.numSamplesPerChip = obj.SampleRate / obj.codeRate;
            % 一个完整C/A码长度(如1023)下的采样点数（即一个码周期的总采样点数）
            obj.numSamplesPerCB = floor(obj.SampleRate * obj.codeLen / obj.codeRate);
            % 初始码相位偏移对应的采样点数：用于精确对齐初始延迟
            numsamprot = round(obj.InitialCodePhaseOffset * obj.numSamplesPerChip);
            % 初始化队列大小，确保初始码相位偏移对应的采样点恰好插入第一个队列
            % 先算需要补齐到最近的码块整数倍，然后减去实际相位偏移表示实际需初始化的采样点数
            obj.dequeSizeInit = ceil(numsamprot / obj.numSamplesPerCB) * obj.numSamplesPerCB - numsamprot;
            % 输入采样队列
            obj.deque = Deque(obj.dequeSizeInit, false, obj.antennaNum);
            % 积分输出样本队列
            obj.deque2 = Deque(round(obj.numCBPerSym / CBRate * obj.SampleRate), true, obj.antennaNum);
        end


        function [result, performationIndex] = stepImpl(obj, u)
            % 执行载波跟踪步骤：剥离载波、鉴别、滤波更新
            % 输入: u - 输入的 I 信号 (N x M 矩阵, N 为样本数, M 为天线数)

            % 输出： [result, performationIndex] = demodulatorModel.step(waveformRx);
            % result 结构体 包括相位误差、频率误差、锁定状态等
            % performationIndex 结构体 包含可能需要的其他中间结果以及性能评估等

            CBRate = obj.codeRate / obj.codeLen;
            numCBPerInteg = round(obj.IntegTime * CBRate); % 每个积分周期内包含的码块数

            numSamplesPerInteg = round(obj.SampleRate * obj.IntegTime);

            dequeSizeEst = round(obj.cph * obj.numSamplesPerChip); % Residual delay in samples
            dequeSize = obj.deque.size();
            obj.deque.push_back(u);


            if dequeSize ~= dequeSizeEst
                dequeSizeDelta = dequeSizeEst - dequeSize;

                if dequeSizeDelta > 0
                    uTmp = obj.deque2.pop_back(dequeSizeDelta);
                    obj.deque.push_front(uTmp);
                    obj.tFrameStart = obj.tFrameStart - dequeSizeDelta / obj.SampleRate;
                else % numextradelay < 0. Equal to zero is not possible because of the first if condition
                    uTmp = obj.deque.pop_front(abs(dequeSizeDelta));
                    obj.deque2.push_back(uTmp);
                    obj.tFrameStart = obj.tFrameStart - dequeSizeDelta / obj.SampleRate;
                end

            end

            result = struct();
            performationIndex = struct();

            % 如果队列中的样本数达到每次积分所需数量
            if obj.deque.size() >= numSamplesPerInteg
                % 从队列头部取出指定数量样本
                u = obj.deque.pop_front(numSamplesPerInteg);
                % 将取出的样本加入第二个队列
                obj.deque2.push_back(u);

                % 0. 更新时间
                steptime = obj.tFrameStart - obj.tFrameStartPrev;   % 上一次至本次帧起始时间差
                tMeasure = obj.tFrameStart + obj.IntegTime / 2;     % 当前测量时刻（积分中心）
                obj.phCF = obj.phCF + 2 * pi * obj.CenterFrequency * steptime; % 累加载波中心频率相位
                % 生成时间向量，用于后续处理
                t = (-numSamplesPerInteg / 2:numSamplesPerInteg / 2 - 1)' / obj.SampleRate;

                % 1. 状态量先验估计
                % 先验相位和频率估计
                delay = mod(- obj.cph, obj.numCBPerSym * obj.codeLen);
                % 当前频率与相位
                fqy = obj.fqy;
                ph = obj.ph + steptime * 2 * pi * obj.fqy;

                % 2. 本地载波生成 与 载波剥离
                % 瞬时载波频率
                fc = obj.CenterFrequency + fqy;
                % 旋转对载波相位的影响估计
                % see 1.2.2.旋转对接收信号载波相位的影响 估计模型
                % obj.rotPhase = obj.rotPhaseModel(t, obj.rotFreq, obj.rotCarrierPartEst);
                obj.rotPhase = zeros(size(u)); 
                % 本地载波相位
                phases = ph + 2 * pi * fc * t + obj.phCF + obj.rotPhase;
                % 载波剥离
                iqsig = u .* exp(-1j * phases);

                % 3. 码相位剥离， 这一部分后续需要整理到  CodeTrackingLoop.m
                % 码相关
                [iq_p, iq_e, iq_l] = obj.pCodeTrackingLoop(iqsig);

                % 4. 以$T_{coh}$ 为周期的积分清除器, 等于对iq_p分段求和
                % integ_coh_p  = obj.integCohP(iq_p, obj.T_coh, obj.SampleRate);
                % integ_coh_e = obj.integCohP(iq_e, obj.T_coh, obj.SampleRate);
                % integ_coh_l = obj.integCohP(iq_l, obj.T_coh, obj.SampleRate);
                % len1 * 天线数量 -> len2 * ? *  天线数量: 这里缺失了一点数据, 但是不影响后续处理?
                samples_per_Tcoh = round(obj.T_coh * obj.SampleRate);
                integ_coh_p = reshape(iq_p(1 : samples_per_Tcoh * floor(length(iq_p) / samples_per_Tcoh), :), samples_per_Tcoh, floor(length(iq_p) / samples_per_Tcoh), obj.antennaNum);
                integ_coh_e = reshape(iq_e(1 : samples_per_Tcoh * floor(length(iq_e) / samples_per_Tcoh), :), samples_per_Tcoh, floor(length(iq_e) / samples_per_Tcoh), obj.antennaNum);
                integ_coh_l = reshape(iq_l(1 : samples_per_Tcoh * floor(length(iq_l) / samples_per_Tcoh), :), samples_per_Tcoh, floor(length(iq_l) / samples_per_Tcoh), obj.antennaNum);

                % 旋转频率估计时间点
                tForRotFreqEstTemp = t(round(samples_per_Tcoh / 2):samples_per_Tcoh:end); % 存在问题，应该是 end -1
                
                % 旋转频率估计，利用 pRotatingFrequencyTracker
                % 利用 pRotatingFrequencyTracker 估计旋转频率， 存在问题
                [integ_coh_p, rotFqyerr] = obj.pRotatingFrequencyTracker.step(integ_coh_p, tForRotFreqEstTemp);
                obj.rotFreq = obj.pRotatingFrequencyTracker.getFreqEst(); % 获取旋转频率估计
                
                % 多天线合并环路，输出合并多天线信号, 未完成，实际输出第一个天线的信号
                [integ_coh_p_cob, integ_coh_e_cob, integ_coh_l_cob] = obj.pMultiAntennaCombiner.step(integ_coh_p, integ_coh_e, integ_coh_l);

                % 旋转载波成分跟踪环路
                % 利用 pRotatingCarrierPartTracker 跟踪旋转载波成分
                % obj.pRotatingCarrierPartTracker.step(integ_coh_p_cob);
                % obj.rotCarrierPartEst = obj.pRotatingCarrierPartTracker.getCarrierPartEst(); % 获取旋转载波成分估计

                % 5. 以$T$ 为周期的积分清除器

                % 6. 码环后续处理, 输入信号为Early/Late支路
                [delayerr, cpherr] = obj.pCodeTrackingLoop.discriminatorEL(integ_coh_e_cob, integ_coh_l_cob, numCBPerInteg);
                % DLL回路滤波器，更新码相位
                cphdelta = obj.pCodeTrackingLoop.loopFilterImpl(cpherr, steptime);
                obj.cph = obj.cph + cphdelta; 


                % 7. 载波环鉴别器
                % 传统FLL判别器输入，得到频率与符号转移量等
                fqyerr = obj.pFreqDiscriminator(integ_coh_p_cob, obj.IntegTime);

                % PLL判别器
                % 相位积分结果
                % integpval = integ_coh_p_cob * patTransEst;
                integpval = sum(integ_coh_p_cob);
                % 相位鉴别器
                pherr = obj.pFreqDiscriminator.PhaseDiscriminator(integpval);

                % 调用辅助C/N估算器
                % cndr = obj.pHelperCNDREstimator(integpval);
                cndr = 0; % 暂设为0

                % FLL回路滤波器，频率更新
                fqydelta = obj.FLLFilter(fqyerr, steptime);
                obj.fqy = fqy + fqydelta; % 后验频率估计

                % PLL回路滤波器，相位更新
                phdelta = obj.PLLFilter(pherr, steptime);
                obj.ph = ph + phdelta; % 后验相位估计

                % 相位锁定探测
                [phlocko, phlockp] = obj.pPLLLockDetector(integpval);

                % 更新时间标签
                obj.tFrameStartPrev = obj.tFrameStart;
                obj.tFrameStart = obj.tFrameStart + numSamplesPerInteg / obj.SampleRate;

                result.frequencyError = fqyerr;
                result.phaseError = pherr;
                result.codeError = delayerr;
                result.frequency = fqy;
                result.phase = ph;
                result.delay = delay;
                result.cndr = cndr;

                result.rotFreqEst = obj.rotFreq;
                result.rotAEst = obj.rotCarrierPartEst.A;
                result.rotAlphaEst = obj.rotCarrierPartEst.alpha;
                result.rotFqyErr = rotFqyerr;

                performationIndex.tMeasure = tMeasure;
                performationIndex.integ_coh_p_cob = integ_coh_p_cob;
                performationIndex.phlocko = phlocko; 
                performationIndex.phlockp = phlockp;
                
            end


        end
    end
end